/*
 * filter.cpp
 *
 *  Created on: 27 Jan 2014
 *      Author: bohni
 */

#include "filter.h"

//////////////////////////////////////////////////////////
#define GYRO_SENSITIVITY		16 		//-+2000 grad/s   LSB pro grad/s
#define GYRO_SENSITIVITY_FLOAT 	16.4
#define ACCEL_SENSITIVITY		8192	// LSB pro g
//////////////////////////////////////////////////////////
void convert_raw_data();

struct sensor_data {
	float rate;
	float bias;
	int16_t accel;
	int16_t angle_accel;
};

struct sensor_data x_achse = { 0, 0, 0, 0 };
struct sensor_data y_achse = { 0, 0, 0, 0 };

struct filter_out x = { 0, 0, 0 };
struct filter_out y = { 0, 0, 0 };
struct filter_out z = { 0, 0, 0 };

uint8_t gyro_percentage = 0;

float gyro_angle_x = 0;
float gyro_angle_y = 0;

float tmp = 0;

int16_t accel_z = 0;

void filter_init() {

	x_achse.bias = (float) xG_off / GYRO_SENSITIVITY_FLOAT;
	y_achse.bias = (float) yG_off / GYRO_SENSITIVITY_FLOAT;

	x_achse.accel = sensor_puffer[3] / 300;
	y_achse.accel = sensor_puffer[4] / 300;
	accel_z = sensor_puffer[5] / 300;

	gyro_percentage = eeprom_read_byte((uint8_t *) EEPROM_ADR_LOW_PASS);

}

void filter_update() {

	convert_raw_data();

	gyro_angle_x = x.angle + x_achse.rate * ABTASTZEIT_IN_MS / 1000.0;
	gyro_angle_y = y.angle + y_achse.rate * ABTASTZEIT_IN_MS / 1000.0;

	tmp = gyro_angle_x * gyro_percentage / 100.0
			+ (100 - gyro_percentage) / 100.0 * x_achse.angle_accel;
	x.angle = tmp;
	tmp = gyro_angle_y * gyro_percentage / 100.0
			+ (100 - gyro_percentage) / 100.0 * y_achse.angle_accel;
	y.angle = tmp;

	x.rate = x_achse.rate;
	y.rate = y_achse.rate;

	z.rate = (sensor_puffer[2] - zG_off) / GYRO_SENSITIVITY_FLOAT;

	z.angle += z.rate * ABTASTZEIT_IN_MS / 1000.0;
}

void filter_coefficent_update() {

	system_pause();
	sei();
	// Allow Interrupts for UART-Receiver

	send_buffer[0] = 0;
	send_buffer[1] = 0;
	send_buffer[2] = 0;
	send_buffer[3] = gyro_percentage;

	UART_send(4);

	while (received_number < 4)
		;

	eeprom_update_byte((uint8_t *) EEPROM_ADR_LOW_PASS, receive_buffer[3]);

	reset_receive_buffer();
	system_reset();
}

void convert_raw_data() {

	x_achse.rate = (float) sensor_puffer[0] / GYRO_SENSITIVITY_FLOAT; // in grad/s
	y_achse.rate = (float) sensor_puffer[1] / GYRO_SENSITIVITY_FLOAT;

	x_achse.rate = x_achse.rate - x_achse.bias;
	y_achse.rate = y_achse.rate - y_achse.bias;

	x_achse.accel = sensor_puffer[3] / 300;
	y_achse.accel = sensor_puffer[4] / 300;
	accel_z = (sensor_puffer[5] + ACCEL_SENSITIVITY) / 300; // plus sensitivity um 1G auszugleichen

	x.acceleration = (float)sensor_puffer[3] / ACCEL_SENSITIVITY * 9.81;
	y.acceleration = (float)sensor_puffer[4] / ACCEL_SENSITIVITY * 9.81;
	z.acceleration = (float)sensor_puffer[5] / ACCEL_SENSITIVITY * 9.81;

	// Winkel über Accelerometer berechnen

	x_achse.angle_accel = arctan_deg(y_achse.accel, accel_z); // in 1 grad
	y_achse.angle_accel = arctan_deg(x_achse.accel, accel_z);
//		x_achse.angle_accel = arctan_approx( y_achse.accel, accel_z ); // in 1 grad
//		y_achse.angle_accel = arctan_approx( x_achse.accel, accel_z );

#ifdef DEBUG_ENABLED
	//DEBUG Werte schreiben

	// ANGLE

	send_buffer[21] = (int8_t) x.angle + 128;
	send_buffer[22] = (int8_t) y.angle + 128;
	send_buffer[23] = (int8_t) z.angle + 128;

	// ACCEL

	send_buffer[12] = x_achse.accel + 128;
	send_buffer[13] = y_achse.accel + 128;
	send_buffer[14] = accel_z + 128;

	// GYRO

	send_buffer[15] = (int8_t) x.rate + 128;
	send_buffer[16] = (int8_t) y.rate + 128;
	send_buffer[17] = (int8_t) z.rate + 128;
#endif
}

